#include "motor.h"

#include "C_code_Log.h"
#include "myconfig.h"
#include "pid.h"
#include "tim.h"
// /* OLD

// * 函数介绍：左右轮子速度的调节
// * 输入参数：lspeed（左轮的占空比）rspeed（右轮的占空比）
// * 输出参数：无
// * 返回值  ：无

// */
// void speedAdjustment(int16_t lspeed,int16_t rspeed)
// {
// 	if(rspeed>0)
// 	{
// 		TIM4->CCR3 = rspeed;         // PB9
// 		TIM4->CCR4 = 0;	  // PB8

// 	}
// 	else
// 	{
// 	  TIM4->CCR4 = -rspeed;
// 		TIM4->CCR3 = 0 ;
// 	}

// 	if(lspeed>0)
//  	{
// 		TIM4->CCR1 =0;	   			//PB7
// 		TIM4->CCR2 = lspeed;	  //PB6
// 	}
// 		else
// 		{
// 	  TIM4->CCR2 = 0;
// 		TIM4->CCR1 = -lspeed;
// 		}

// }
/// NEW
// 规划：1、有多种驱动模式
// 1、编码器速度固定输出，用PID调节，优点：速度稳定，可以更换电机，缺点：到达目标速度时间较长（3个时间周期，一个为0.1秒），不适合快速变速，需要定时器
// 2、PWM固定输出，优点是速度快，兼容旧代码，缺点是左右轮速度不一样，直走不正，要手动调整左右的PWM差值，不需要定时器
// 3、PWM给定目标值，但是会用编码器调整4个电机速度或者左右两边速度一致，相当于PWM固定输出的改良版，需要定时器
motorSpeedTypeDef gl_motor;
// motorPWMTypeDef gl_motorPWM;
// 电机PID调节观察者，会在定时器6中断中调用，但是要把Update函数改成自己想要的功能
Observer MotorPIDAdjustObserver = {.id = 2,
                                   .update = NULL /*SpeedAdjustment_PID*/};
/// @brief 电机初始化
/// @note 开启PWM通道，把电机PID调节观察者加入定时器6中断中
/// @warning 此时观察者的update函数为空，需要在运行时修改
void MotorInit() {
  // 开启左前轮PWM通道输出
  HAL_TIM_PWM_Start(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_2);
  // 开启左后轮PWM通道输出
  HAL_TIM_PWM_Start(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_2);
  // 开启右前轮PWM通道输出
  HAL_TIM_PWM_Start(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_2);
  // 开启右后轮PWM通道输出
  HAL_TIM_PWM_Start(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_2);

  gl_TIM6_IT.add(&gl_TIM6_IT, &MotorPIDAdjustObserver);
}

/// @brief 设置电机结构体的速度
/// @param motor 电机结构体
/// @param leftSpeed 左轮速度
/// @param rightSpeed 右轮速度
/// @note 只是改变结构体的设定速度，不会改变实际速度
static void setTypeDefSpeed(motorSpeedTypeDef *motor, int16_t leftSpeed,
                     int16_t rightSpeed) {
  motor->leftSpeed = leftSpeed;
  motor->rightSpeed = rightSpeed;
}

/// @brief 用于设置电机的实际速度(通过PWM)
/// @param PWMLeftFront 要设置的左前轮速度（一个PWM值）范围是-8000到8000
/// @param PWMLeftBack 要设置的左后轮速度（一个PWM值）范围是-8000到8000
/// @param PWMRightFort 要设置的右前轮速度（一个PWM值）范围是-8000到8000
/// @param PWMRightBack 要设置的右后轮速度（一个PWM值）范围是-8000到8000
/// @note 可以根据宏定义来改变电机的正方向，可以不用改变电机的接线
/// @note
/// 产生电机驱动的PWM的定时器说明：TIM5通道3和4对应左前，TIM5通道1和2对应左后
/// @note 2023/1/9 已添加4个轮子，未测试
/// @date 2023/11/11
static void setRealSpeed(int16_t PWMLeftFront, int16_t PWMLeftBack,
                         int16_t PWMRightFort, int16_t PWMRightBack) {
  // 限幅
  PWMLeftFront = limit(PWMLeftFront, -8000, 8000);
  PWMLeftBack = limit(PWMLeftBack, -8000, 8000);
  PWMRightFort = limit(PWMRightFort, -8000, 8000);
  PWMRightBack = limit(PWMRightBack, -8000, 8000);

#if LEFT_FRONT_CHANGE_DIRECTION == 0
  if (PWMLeftFront >= 0) {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_1,
                         PWMLeftFront);
  } else {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_2,
                         -PWMLeftFront);
  }
#else
  if (PWMLeftFront < 0) {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_1,
                         PWMLeftFront);
  } else {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_FRONT_TIM_CHANNEL_2,
                         -PWMLeftFront);
  }
#endif
#if LEFT_BACK_CHANGE_DIRECTION == 0
  if (PWMLeftBack >= 0) {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_1,
                         PWMLeftBack);
  } else {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_2,
                         -PWMLeftBack);
  }
#else
  if (PWMLeftBack < 0) {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_1,
                         PWMLeftBack);
  } else {
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&LEFT_TIM_HANDLE, LEFT_BACK_TIM_CHANNEL_2,
                         -PWMLeftBack);
  }
#endif
#if RIGHT_FRONT_CHANGE_DIRECTION == 0
  if (PWMRightFort >= 0) {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_1,
                         PWMRightFort);
  } else {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_2,
                         -PWMRightFort);
  }
#else
  if (PWMRightFort < 0) {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_1,
                         PWMRightFort);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_2, 0);
  } else {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_FRONT_TIM_CHANNEL_2,
                         -PWMRightFort);
  }
#endif
#if RIGHT_BACK_CHANGE_DIRECTION == 0
  if (PWMRightBack >= 0) {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_1,
                         PWMRightBack);
  } else {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_2,
                         -PWMRightBack);
  }
#else
  if (PWMRightBack < 0) {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_2, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_1,
                         PWMRightBack);
  } else {
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_1, 0);
    __HAL_TIM_SetCompare(&RIGHT_TIM_HANDLE, RIGHT_BACK_TIM_CHANNEL_2,
                         -PWMRightBack);
  }
#endif
}

static PIDTypeDef pidLeftFront;   // 左前轮PID结构体
static PIDTypeDef pidLeftBack;    // 左后轮PID结构体
static PIDTypeDef pidRightFront;  // 右前轮PID结构体
static PIDTypeDef pidRightBack;   // 右后轮PID结构体

/// @brief 电机固定转速模式
/// @note
/// 第一种驱动模式，编码器速度固定输出，用PID调节，优点：速度稳定，可以更换电机，缺点：到达目标速度时间较长（3个时间周期，一个为0.1秒），不适合快速变速
/// @param motor 电机结构体
/// @note 只对左前轮进行PID调节，未完成
/// @note 新增 2024/1/8 未做完全测试
/// 增加了电机插反检测，如果电机插反了，会停止电机，方法：通过检查连续两次的误差是否在减小来判断电机是否在正确的方向上运行。
/// 如果误差在增大，那么我们可以认为电机可能在错误的方向上运行
void SpeedAdjustment_PID() {
  static uint8_t flag = 0;
  static int16_t lastError[4] = {0};    // 用于存储上一次的误差
  static uint8_t errorNum[4] = {0};     // 连续出现误差的次数
  static uint8_t reverseFlag[4] = {0};  // 电机正负反插标志位
  int PWMLeftFront = 0;
  int PWMLeftBack = 0;
  int PWMRightFront = 0;
  int PWMRightBack = 0;
  for (int wheel = 0; wheel < 4; wheel++) {
    if (reverseFlag[wheel] == 1) {
      motorStop();
      LOG("ERROR", "第%d个电机插反了,无法进行PID调节", wheel);
      return;
    }
  }

  // 保证只初始化一次
  if (flag == 0) {
    flag = 1;
    // 限幅8000，比例2，积分8，微分0，主要靠积分,此参数适用于0.1秒调整一次
    PID_Init(&pidLeftFront, gl_motor.leftSpeed, 6000, 2, 8, 0);
    // 限幅一定不能超过8k
    PID_Init(&pidLeftBack, gl_motor.leftSpeed, 6000, 2, 8, 0);
    PID_Init(&pidRightFront, gl_motor.rightSpeed, 6000, 2, 8, 0);
    PID_Init(&pidRightBack, gl_motor.rightSpeed, 6000, 2, 8, 0);
    lastError[0] = gl_motor.encoderLeftFront - gl_motor.leftSpeed;
    lastError[1] = gl_motor.encoderLeftBack - gl_motor.leftSpeed;
    lastError[2] = gl_motor.encoderRightFront - gl_motor.rightSpeed;
    lastError[3] = gl_motor.encoderRightBack - gl_motor.rightSpeed;
  }
  // 计算当前误差
  int16_t currentError[4] = {gl_motor.encoderLeftFront - gl_motor.leftSpeed,
                             gl_motor.encoderLeftBack - gl_motor.leftSpeed,
                             gl_motor.encoderRightFront - gl_motor.rightSpeed,
                             gl_motor.encoderRightBack - gl_motor.rightSpeed};

  // 电机插反检测
  for (int wheel = 0; wheel < 4; wheel++) {
    if (abs16(currentError[wheel]) > abs16(lastError[wheel]) + 10) {
      errorNum[wheel]++;
      if (errorNum[wheel] >= 3) {
        reverseFlag[wheel] = 1;
        motorStop();
        LOG("ERROR", "电机速度调节当前误差大于上一次误差,可能电机插反了");
        return;
      }
    } else {
      errorNum[wheel] = 0;
    }
    lastError[wheel] = currentError[wheel];  // 更新上一次的误差
  }

  // 电机没开检测，应该可以这么写，没测试
  //  else if(abs16(currentError) == abs16(lastError))
  //  {
  //    errorNum++;
  //    if(errorNum>=3)
  //    {
  //      LOG("ERROR", "电机速度调节当前误差等于上一次误差,可能电机没开");
  //    }
  //  }

  // 编码器计数器和设定速度的差值大于5才进行PID调节，防止抖动
  if (abs16(currentError[0]) > 5) {
    PWMLeftFront =
        positionPIDCalc_LimitSumError(&pidLeftFront, gl_motor.encoderLeftFront);
  }
  if (abs16(currentError[1]) > 5) {
    PWMLeftBack =
        positionPIDCalc_LimitSumError(&pidLeftBack, gl_motor.encoderLeftBack);
  }
  if (abs16(currentError[2]) > 5) {
    PWMRightFront = positionPIDCalc_LimitSumError(&pidRightFront,
                                                  gl_motor.encoderRightFront);
  }
  if (abs16(currentError[3]) > 5) {
    PWMRightBack =
        positionPIDCalc_LimitSumError(&pidRightBack, gl_motor.encoderRightBack);
  }

  // 感觉有问题
  //  // 的正负号和gl_motor.encoder的正负号相同，避免电机突然反向转动
  //  if (!(abs16(gl_motor.encoderLeftFront) < 10 ||
  //        (PWMLeftFront & 0x8000) == (gl_motor.encoderLeftFront & 0x8000))) {
  //    PWMLeftFront = 0;
  //    pidLeftFront.sumError = 0;
  //  }
  //  if (!(abs16(gl_motor.encoderLeftBack) < 10 ||
  //        (PWMLeftBack & 0x8000) == (gl_motor.encoderLeftBack & 0x8000))) {
  //    PWMLeftBack = 0;
  //    pidLeftBack.sumError = 0;
  //  }
  //  if (!(abs16(gl_motor.encoderRightFront) < 10 ||
  //        (PWMRightFront & 0x8000) == (gl_motor.encoderRightFront & 0x8000)))
  //        {
  //    PWMRightFront = 0;
  //    pidRightFront.sumError = 0;
  //  }
  //  if (!(abs16(gl_motor.encoderRightBack) < 10 ||
  //        (PWMRightBack & 0x8000) == (gl_motor.encoderRightBack & 0x8000))) {
  //    PWMRightBack = 0;
  //    pidRightBack.sumError = 0;
  //  }

  // 输出 PWM
  setRealSpeed(PWMLeftFront, PWMLeftBack, PWMRightFront, PWMRightBack);
}
/// @brief 用于在运行时修改电机速度的目标值,在电机固定转速模式下使用
/// @param leftSpeed 左轮的目标速度
/// @param rightSpeed 右轮的目标速度
/// @note 未完成
/// @note
/// 不能重新初始化，那样会重新从0开始输出PWM，会导致电机转速突然变小，在运行时想修改电机速度要用这个函数
/// @date 2023/11/12
void resetMotorSpeedValue_runTime(int16_t leftSpeed, int16_t rightSpeed) {
  if(getMotorRunMode()!=MOTOR_MODE_SPEED)
  {
    LOG("ERROR","电机不是固定转速模式,无法使用resetMotorSpeedValue_runTime函数");
    return;
  }
  gl_motor.leftSpeed = leftSpeed;
  pidLeftFront.setValue = leftSpeed;
  pidLeftBack.setValue = leftSpeed;
  gl_motor.rightSpeed = rightSpeed;
  pidRightFront.setValue = rightSpeed;
  pidRightBack.setValue = rightSpeed;
}
/// @brief 用于对旧代码的支持，直接设置电机的速度
/// @param leftPWM 左轮的速度
/// @param rightPWM 右轮的速度
void speedAdjustment(int16_t leftPWM, int16_t rightPWM) {
  if (MotorPIDAdjustObserver.update == NULL) {
    setRealSpeed(leftPWM, leftPWM, rightPWM, rightPWM);
  } else {
    LOG("ERROR", "电机已经处于固定转速模式,无法使用speedAdjustment函数");
  }
}
/*
 * 函数介绍：调整左右电机的速度差，用于位置式PID
 * 输入参数：leftSpeed(初始左轮速度)，rightSpeed（初速右轮速度），dSpeed（偏差速度）
 * 输出参数：无
 * 返回值  ：无
 * 其他    ：实现让小车能够在设定的速度下稳定跑直线
 */
void positionSpeedOut(int leftSpeed, int rightSpeed, int dSpeed) {
  dSpeed = dSpeed / 2;
  leftSpeed -= dSpeed;
  rightSpeed += dSpeed;

  speedAdjustment(leftSpeed, rightSpeed);
}

/// @brief 电机停止转动
/// @note 电机停止使用这个函数，为以后使用无刷电机做铺垫
void motorStop() {
  motorRunModeChange(MOTOR_MODE_Direct);
  setRealSpeed(0, 0, 0, 0);
  gl_motor.leftSpeed = 0;
  gl_motor.rightSpeed = 0;
}

/// @brief 改变电机的驱动模式
/// @param 电机的驱动模式，目前有两种，一种是直接输出PWM，一种是固定转速模式
void motorRunModeChange(enum MotorMode mode) {
  switch (mode) {
    case MOTOR_MODE_Direct:
      MotorPIDAdjustObserver.update = NULL;
      break;
    case MOTOR_MODE_SPEED:
      MotorPIDAdjustObserver.update = SpeedAdjustment_PID;
      break;
    default:
      break;
  }
}
enum MotorMode getMotorRunMode() {
  if (MotorPIDAdjustObserver.update == NULL) {
    return MOTOR_MODE_Direct;
  } else {
    return MOTOR_MODE_SPEED;
  }
}
// 编码器部分
/// @brief 获取编码器的值，用于计算速度和累计距离
/// @note 未完成
/// @note
/// 算出来的速度是每0.1秒的速度，累计距离是每0.1秒的累计距离，即使调用的频率不是0.1秒一次，也要这样算，才能用于PID调节
void GET_NUM() {
  static uint8_t callFlag = 0;         // 用于判断是否是第一次调用
  static uint32_t lastTime_hight = 0;  // 全局时间的值
  static uint32_t lastTime_low = 0;    // timer6的CNT值

  uint32_t currentTime_hight = getGlobalTime();
  uint32_t currentTime_low = __HAL_TIM_GET_COUNTER(&TIM_HANDLE_GLOBAL_TIME);

  if (callFlag == 0 || currentTime_hight - lastTime_hight >
                           100) {  // 如果是第一次调用或者超过1秒没有调用
    callFlag = 1;                  // 标记为已调用
  } else {
    uint32_t interval_hight = currentTime_hight - lastTime_hight;
    uint32_t interval_sum =
        interval_hight * 100 + currentTime_low - lastTime_low;  // 单位1/10000秒
    gl_motor.encoderLeftFront = ((double)((int16_t)__HAL_TIM_GET_COUNTER(
                                    &LEFT_FRONT_ENCODER_TIM_HANDLE))) /
                                interval_sum * 1000;
    gl_motor.encoderLeftBack = ((double)((int16_t)__HAL_TIM_GET_COUNTER(
                                   &LEFT_BACK_ENCODER_TIM_HANDLE))) /
                               interval_sum * 1000;
    gl_motor.encoderRightFront = ((double)((int16_t)__HAL_TIM_GET_COUNTER(
                                     &RIGHT_FRONT_ENCODER_TIM_HANDLE))) /
                                 interval_sum * 1000;
    gl_motor.encoderRightBack = ((double)((int16_t)__HAL_TIM_GET_COUNTER(
                                    &RIGHT_BACK_ENCODER_TIM_HANDLE))) /
                                interval_sum * 1000;
  }

  // 更新lastTime
  lastTime_hight = currentTime_hight;
  lastTime_low = currentTime_low;

  // LOG("INFO","gl_motor.encoderLeftFront = %d",gl_motor.encoderLeftFront);
  gl_motor.encoderDistance += ((int_least16_t)gl_motor.encoderLeftFront +
                               (int_least16_t)gl_motor.encoderLeftBack +
                               (int_least16_t)gl_motor.encoderRightFront +
                               (int_least16_t)gl_motor.encoderRightBack) /
                              4;
  // LOG("INFO","gl_motor.encoderDistance = %d",gl_motor.encoderDistance);
  __HAL_TIM_SET_COUNTER(&LEFT_FRONT_ENCODER_TIM_HANDLE, 0);
  __HAL_TIM_SET_COUNTER(&LEFT_BACK_ENCODER_TIM_HANDLE, 0);
  __HAL_TIM_SET_COUNTER(&RIGHT_FRONT_ENCODER_TIM_HANDLE, 0);
  __HAL_TIM_SET_COUNTER(&RIGHT_BACK_ENCODER_TIM_HANDLE, 0);
}
Observer encoderNumObserver = {.id = 1, .update = GET_NUM};  // 编码器中断观察者
void EncoderInit() {
  HAL_TIM_Encoder_Start(&LEFT_FRONT_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&LEFT_BACK_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&RIGHT_FRONT_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&RIGHT_BACK_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  gl_TIM6_IT.add(&gl_TIM6_IT, &encoderNumObserver);
}
